Constrained Optimization Path Following of Wheeled Robots in Natural Terrain

A smooth-primitive constrained-optimization-based path-tracking algorithm for mobile robots that compensates for rough terrain, predictable vehicle dynamics, and vehicle mobility constraints has been developed, implemented, and tested on the DARPA LAGR platform. Traditional methods for the geometric path following control problem involve trying to meet position constraints at fixed or velocity dependent look-ahead distances using arcs. We have reformulated the problem as an optimal control problem, using a trajectory generator that can meet arbitrary boundary state constraints. The goal state along the target path is determined dynamically by minimizing a utility function based on corrective trajectory feasibility and cross-track error. A set of field tests compared the proposed method to an implementation of the pure pursuit algorithm and showed that the smooth corrective trajectory constrained optimization approach exhibited higher performance than pure pursuit by achieving rough four times lower average cross-track error and two times lower heading error.